{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "9e2f1ea165e14a92971a0e6fd74f8206",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Plot(antialias=3, axes=['x', 'y', 'z'], background_color=16777215, camera=[35.874242529278916, -24.12575747072…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "\"\"\"Solve a random maze with Markovian Decision Process\"\"\"\n",
    "# -----------------------------------------------------------------------------\n",
    "# Copyright 2019 (C) Nicolas P. Rougier & Anthony Strock\n",
    "# Released under a BSD two-clauses license\n",
    "#\n",
    "# References: Bellman, Richard (1957), A Markovian Decision Process.\n",
    "#             Journal of Mathematics and Mechanics. Vol. 6, No. 5.\n",
    "# -----------------------------------------------------------------------------\n",
    "#https://github.com/rougier/ML-Recipes/blob/master/recipes/MDP/value-iteration.py\n",
    "#https://en.wikipedia.org/wiki/Markov_decision_process\n",
    "import numpy as np\n",
    "from scipy.ndimage import generic_filter\n",
    "\n",
    "def maze(shape=(30, 50), complexity=0.8, density=0.8):\n",
    "    shape = (np.array(shape)//2)*2 + 1\n",
    "    n_complexity = int(complexity*(shape[0]+shape[1]))\n",
    "    n_density = int(density*(shape[0]*shape[1]))\n",
    "    Z = np.ones(shape, dtype=bool)\n",
    "    Z[1:-1, 1:-1] = 0\n",
    "    P = (np.dstack([np.random.randint(0, shape[0]+1, n_density),\n",
    "                    np.random.randint(0, shape[1]+1, n_density)])//2)*2\n",
    "    for (y,x) in P.squeeze():\n",
    "        Z[y, x] = 1\n",
    "        for j in range(n_complexity):\n",
    "            neighbours = []\n",
    "            if x > 1:           neighbours.append([(y, x-1), (y, x-2)])\n",
    "            if x < shape[1]-2:  neighbours.append([(y, x+1), (y, x+2)])\n",
    "            if y > 1:           neighbours.append([(y-1, x), (y-2, x)])\n",
    "            if y < shape[0]-2:  neighbours.append([(y+1, x), (y+2, x)])\n",
    "            if len(neighbours):\n",
    "                next_1, next_2 = neighbours[np.random.randint(len(neighbours))]\n",
    "                if Z[next_2] == 0:\n",
    "                    Z[next_1] = Z[next_2] = 1\n",
    "                    y, x = next_2\n",
    "            else:\n",
    "                break\n",
    "    return Z\n",
    "\n",
    "def solve(Z, start, goal):\n",
    "    Z = 1 - Z\n",
    "    G = np.zeros(Z.shape)\n",
    "    G[start] = 1\n",
    "\n",
    "    # We iterate until value at exit is > 0. This requires the maze\n",
    "    # to have a solution or it will be stuck in the loop.\n",
    "    def diffuse(Z, gamma=0.99):\n",
    "        return max(gamma*Z[0], gamma*Z[1], Z[2], gamma*Z[3], gamma*Z[4])\n",
    "\n",
    "    while G[goal] == 0.0:\n",
    "        G = Z * generic_filter(G, diffuse, footprint=[[0, 1, 0],\n",
    "                                                      [1, 1, 1],\n",
    "                                                      [0, 1, 0]])\n",
    "\n",
    "    # Descent gradient to find shortest path from entrance to exit\n",
    "    y, x = goal\n",
    "    dirs = (0,-1), (0,+1), (-1,0), (+1,0)\n",
    "    P = []\n",
    "    while (x, y) != start:\n",
    "        P.append((y,x))\n",
    "        neighbours = [-1, -1, -1, -1]\n",
    "        if x > 0:            neighbours[0] = G[y, x-1]\n",
    "        if x < G.shape[1]-1: neighbours[1] = G[y, x+1]\n",
    "        if y > 0:            neighbours[2] = G[y-1, x]\n",
    "        if y < G.shape[0]-1: neighbours[3] = G[y+1, x]\n",
    "        a = np.argmax(neighbours)\n",
    "        x, y  = x + dirs[a][1], y + dirs[a][0]\n",
    "    P.append((y,x))\n",
    "\n",
    "    return P, G\n",
    "\n",
    "def showSolution3D(S, start, goal):\n",
    "    from vtkplotter import Text, Cube, Line, Grid, merge, show\n",
    "\n",
    "    pts, cubes, txts = [], [], []\n",
    "    pts = [(x,-y) for y,x in S[0]]\n",
    "    for y,line in enumerate(Z):\n",
    "        for x,c in enumerate(line):\n",
    "            if c: cubes.append(Cube([x,-y,0]))\n",
    "\n",
    "    path = Line(pts).lw(6).c('tomato')\n",
    "    walls = merge(cubes).texture('metal2')\n",
    "\n",
    "    sy, sx = S[1].shape\n",
    "    gradient = np.flip(S[1], axis=0).ravel()\n",
    "    grd = Grid(pos=((sx-1)/2, -(sy-1)/2, -0.49), sx=sx, sy=sy, resx=sx, resy=sy)\n",
    "    grd.wireframe(False).cellColors(gradient, cmap='gist_earth_r')\n",
    "    grd.addScalarBar(title='Gradient', horizontal=True)\n",
    "\n",
    "    return show(path, walls, grd, bg='white', axes=0)\n",
    "\n",
    "################################################################################\n",
    "np.random.seed(4)\n",
    "\n",
    "Z = maze(shape=(50, 70))\n",
    "\n",
    "start, goal = (1,1), (Z.shape[0]-2, Z.shape[1]-2)\n",
    "S = solve(Z, start, goal)\n",
    "\n",
    "showSolution3D(S, start, goal)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
